package net.minhdiep.compassview;

import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.util.Log;
import android.view.Display;
import android.view.Surface;
import android.view.WindowManager;
import android.widget.TextView;
import android.widget.Toast;
import net.minhdiep.compassview.view.CompassView;

import java.math.BigDecimal;


/*
 * User: Minh Diep
 * Date: 6/8/13
 * Time: 1:17 AM
 */

public class CompassViewActivity extends Activity implements SensorEventListener {

    private CompassView compassView;
    private static SensorManager mSensorManager;
    private Sensor sensor;
    private Sensor accelerometer;
    private Sensor magnetometer;
    float[] aValues = new float[3];
    float[] mValues = new float[3];
    private TextView degreeTextView;
    private int rotation;

    public void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_compass_view);
        compassView = (CompassView) findViewById(R.id.compass_view);
        degreeTextView = (TextView) findViewById(R.id.degree_tv);
        mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
        accelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        magnetometer = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);

        String windoSrvc = Context.WINDOW_SERVICE;
        WindowManager wm = ((WindowManager) getSystemService(windoSrvc));
        Display display = wm.getDefaultDisplay();
        rotation = display.getRotation();

        updateCompassOrientation(new float[]{0, 0, 0});
    }

    private void updateCompassOrientation(float values[]) {
        if (compassView != null) {
            compassView.setBearing(values[0]);
            compassView.setPitch(values[1]);
            compassView.setRoll(-values[2]);
            compassView.invalidate();
        }
    }

    private float[] calculateOrientation() {
        float[] values = new float[3];
        float[] R = new float[9];
        float[] outR = new float[9];

        SensorManager.getRotationMatrix(R, null, aValues, mValues);

        // Remap the coordinates based on the natural device orientation.
        int x_axis = SensorManager.AXIS_X;
        int y_axis = SensorManager.AXIS_Y;
        switch (rotation) {
            case (Surface.ROTATION_90):
                x_axis = SensorManager.AXIS_Y;
                y_axis = SensorManager.AXIS_MINUS_X;
                break;
            case (Surface.ROTATION_180):
                y_axis = SensorManager.AXIS_MINUS_Y;
                break;
            case (Surface.ROTATION_270):
                x_axis = SensorManager.AXIS_MINUS_Y;
                y_axis = SensorManager.AXIS_X;
                break;
            default:
                break;
        }
        SensorManager.remapCoordinateSystem(R, x_axis, y_axis, outR);
        SensorManager.getOrientation(outR, values);

        // Convert from Radians to Degrees.
        values[0] = round((float) Math.toDegrees(values[0]), 2);
        values[1] = (float) Math.toDegrees(values[1]);
        values[2] = (float) Math.toDegrees(values[2]);

        return values;
    }

    @Override
    protected void onResume() {
        super.onResume();
//        mSensorManager.registerListener(this, sensor, SensorManager.SENSOR_DELAY_UI);
        mSensorManager.registerListener(this, accelerometer, SensorManager.SENSOR_DELAY_UI);
        mSensorManager.registerListener(this, magnetometer, SensorManager.SENSOR_DELAY_UI);
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        if (sensor != null) {
            mSensorManager.unregisterListener(this);
        }
    }

    static final float ALPHA = 0.05f;

    protected float[] lowPass(float[] input, float[] output) {
        if (output == null) return input;

        for (int i = 0; i < input.length; i++) {
            output[i] = output[i] + ALPHA * (input[i] - output[i]);
        }
        return output;
    }

    private float[] highPass(float[] prev_input, float[] input, float[] output){
        if (output == null) return input;

        for (int i = 0; i < input.length; i++) {
            output[i] = ALPHA * (output[i] + input[i] - prev_input[i]);
        }

        return output;

    }

    public static float round(float d, int decimalPlace) {
        BigDecimal bd = new BigDecimal(Float.toString(d));
        bd = bd.setScale(decimalPlace, BigDecimal.ROUND_HALF_UP);
        return bd.floatValue();
    }

    @Override
    public void onSensorChanged(SensorEvent sensorEvent) {
        if (sensorEvent.sensor.getType() == Sensor.TYPE_ACCELEROMETER){
            aValues = lowPass(sensorEvent.values, aValues);
        }
        if (sensorEvent.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
            mValues = sensorEvent.values;
        float result[] = calculateOrientation();
        updateCompassOrientation(result);
        degreeTextView.setText(String.valueOf(result[0]));
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int i) {

    }
}